#ifndef RRT_PLANNER_H
#define RRT_PLANNER_H

#include "Collision_detector.h"
#include "Flann_wrapper.h"
#include <time.h>
class Rrt_planner
{
public:
	Rrt_planner (Collision_detector* col, Point start, Point target, int iteration_number, int connect_target_rate):
	  m_start(start), m_target(target), m_iteration_number(iteration_number), 
		  m_connect_target_rate(connect_target_rate), m_collision(col)
	{
		time_t seed = time(NULL);
		srand ((unsigned int) seed );
	}


	// runs the construction of RRT and returns a path
	list<Point> run()
	{
		// update structure of start and target vertices. 
		// note that the target is not added to the nearest-neighbor structure
		add_vertex(m_start, true);
		m_start.set_index(0);
		add_vertex(m_target, false);
		m_target.set_index(1);

		for (int i = 0; i < m_iteration_number; i++)
		{
			// check whether an attempt to connect the tree to the target should be made
			bool target_connection = ((i % m_connect_target_rate) == 0);
			
			// draw a random sample or try to extend towards the target
			Point point_rand;
			if (target_connection)
				point_rand = m_target;
			else 
				point_rand = sample_configuration();

			// find the nearest neighbor in tree
			Point point_nearest = nearest(point_rand);

			// try to move toward the new sample from the nearest neighbor
			Point point_new = steer(point_nearest, point_rand);
			FILE* fOutput = fopen("debug_end.txt", "a+");
			fprintf(fOutput, "steer \n(%f , %f) : %f\n", point_new[0],point_new[1],point_new[2]);
			fclose(fOutput);
			// if we managed to move forward add the new vertex to the tree
			if (point_new != point_nearest)
			{
				// if this vertex was not considered before (its id is -1)
				
				if(point_new[0] == m_target[0] && point_new[1] == m_target[1] && point_new[2] != m_target[2] )
				{
				/*	FILE* fOutput = fopen("debug_end.txt", "w+");
					fprintf(fOutput, "(%f , %f) : %f\n : curr %f\n", point_new[0],point_new[1],m_target[2]*360,point_new[2]*360);
					fprintf(fOutput, "return m_target\n");
					fclose(fOutput);*/

					add_vertex(point_new, true);
					point_new = m_id_to_point.back();
					add_edge(point_nearest, point_new);
				}
				else if (get_id(point_new) == -1)
				{
					FILE* fOutput = fopen("debug_end.txt", "a+");
					fprintf(fOutput, "int id == -1 \n(%f , %f) : %f\n", point_new[0],point_new[1],point_new[2]);
					fclose(fOutput);
					add_vertex(point_new, true);
					point_new = m_id_to_point.back(); // to make sure that we point_new has a proper id
					add_edge(point_nearest, point_new);
					continue;
				}

				/*	otherwise, we managed to reach the target position, as this is the only 
					configuration with ID that steer can reach	*/

				if(point_new[2] != m_target[2])
				{
					add_edge(point_new, m_target);
				}
				return get_path();
			}
		}
		return list<Point>();
	}

private:

	/*
		Sample a configuration, which doesn't have 
		to be free.
	*/
	Point	sample_configuration()
	{
		double x = ((double)rand())/((double)RAND_MAX) * 2 - 1;
		double y = ((double)rand())/((double)RAND_MAX) * 2 - 1;
		double theta = ((double)rand())/((double)RAND_MAX) * 2 - 1;

		Point result(x,y,theta);

		return result;
	}

	/*
		Finds the nearest point to the sample
	*/
	Point	nearest(Point p)
	{
		return m_nn_search.nearest_neighbor(p);
	}

	Point	steer(Point s, Point t)
	{
		return m_collision->steer(s,t);
	}

	// recovers the path between the start and target
	list<Point>	get_path()
	{
		list<Point> result;
		result.push_back(m_target);

		while (get_id(result.back()) != get_id(m_start))
		{
			int current_id = get_id(result.back());
			int parent_id = m_parent_adjacency[current_id];
			result.push_back(m_id_to_point[parent_id]);
		}

		result.reverse();
		return result;
	}


	/*	Adds a new vertex to the tree, updates its index and 
		adds the new vertex to the nearest-neighbor search structure. */
	void	add_vertex(Point p, bool add_to_nn)
	{
		p.set_index(m_id_to_point.size());
		m_id_to_point.push_back(p);
		m_parent_adjacency.push_back(-1);
	
		if (add_to_nn)
			m_nn_search.insert(p);
	}

	// Adds an edge between the two vertices
	void			add_edge(Point s, Point t)
	{
		m_parent_adjacency[get_id(t)] = get_id(s);
	}

	// given a point returns its id
	int				get_id(Point p)
	{
		return p.index();
	}

	// given an id returns a point
	Point			get_point(int point_id)
	{
		return m_id_to_point[point_id];
	}	
	
	// parameters
	int m_iteration_number;		// number of iterations of algorithm
	int m_connect_target_rate;	// the rate of attempting to connect target to tree 

	Point m_start,m_target;     // start and target configurations

	Flann_wrapper m_nn_search;  // nearest-neighbors search structure

	vector<Point>							m_id_to_point;

	Collision_detector*						m_collision;

	vector<int>								m_parent_adjacency; // for every vertex stores its parent in the tree
};

#endif